/*
 * MathHelper.c
 *
 *  Created on: Mar 30, 2010
 *      Author: Toro
 */

typedef union FixedU10_6 {
	unsigned int full;
	struct part10_6 {
		unsigned int fraction: 6;
		unsigned int integer: 10;
	} part;
} Fixed10_6 ;

//typedef union Fixed6_10 {
//};

//TODO make template class
struct Vector2 {
	int x;
	int y;
};

struct Vector2_FixedU10_6 {
	Fixed10_6 x;
	Fixed10_6 y;
};

//TODO MAYBE...this LUT can be cut in half, since
//the negative values are symmetrical
//This LUT is WAYYYYY to big for program data, move to memory?
static const int sinTable[256] PROGMEM = {
		0 ,
		402 ,
		804 ,
		1205 ,
		1606 ,
		2006 ,
		2404 ,
		2801 ,
		3196 ,
		3590 ,
		3981 ,
		4370 ,
		4756 ,
		5139 ,
		5520 ,
		5897 ,
		6270 ,
		6639 ,
		7005 ,
		7366 ,
		7723 ,
		8076 ,
		8423 ,
		8765 ,
		9102 ,
		9434 ,
		9760 ,
		10080 ,
		10394 ,
		10702 ,
		11003 ,
		11297 ,
		11585 ,
		11866 ,
		12140 ,
		12406 ,
		12665 ,
		12916 ,
		13160 ,
		13395 ,
		13623 ,
		13842 ,
		14053 ,
		14256 ,
		14449 ,
		14635 ,
		14811 ,
		14978 ,
		15137 ,
		15286 ,
		15426 ,
		15557 ,
		15679 ,
		15791 ,
		15893 ,
		15986 ,
		16069 ,
		16143 ,
		16207 ,
		16261 ,
		16305 ,
		16340 ,
		16364 ,
		16379 ,
		16384 ,
		16379 ,
		16364 ,
		16340 ,
		16305 ,
		16261 ,
		16207 ,
		16143 ,
		16069 ,
		15986 ,
		15893 ,
		15791 ,
		15679 ,
		15557 ,
		15426 ,
		15286 ,
		15137 ,
		14978 ,
		14811 ,
		14635 ,
		14449 ,
		14256 ,
		14053 ,
		13842 ,
		13623 ,
		13395 ,
		13160 ,
		12916 ,
		12665 ,
		12406 ,
		12140 ,
		11866 ,
		11585 ,
		11297 ,
		11003 ,
		10702 ,
		10394 ,
		10080 ,
		9760 ,
		9434 ,
		9102 ,
		8765 ,
		8423 ,
		8076 ,
		7723 ,
		7366 ,
		7005 ,
		6639 ,
		6270 ,
		5897 ,
		5520 ,
		5139 ,
		4756 ,
		4370 ,
		3981 ,
		3590 ,
		3196 ,
		2801 ,
		2404 ,
		2006 ,
		1606 ,
		1205 ,
		804 ,
		402 ,
		0 ,
		-402 ,
		-804 ,
		-1205 ,
		-1606 ,
		-2006 ,
		-2404 ,
		-2801 ,
		-3196 ,
		-3590 ,
		-3981 ,
		-4370 ,
		-4756 ,
		-5139 ,
		-5520 ,
		-5897 ,
		-6270 ,
		-6639 ,
		-7005 ,
		-7366 ,
		-7723 ,
		-8076 ,
		-8423 ,
		-8765 ,
		-9102 ,
		-9434 ,
		-9760 ,
		-10080 ,
		-10394 ,
		-10702 ,
		-11003 ,
		-11297 ,
		-11585 ,
		-11866 ,
		-12140 ,
		-12406 ,
		-12665 ,
		-12916 ,
		-13160 ,
		-13395 ,
		-13623 ,
		-13842 ,
		-14053 ,
		-14256 ,
		-14449 ,
		-14635 ,
		-14811 ,
		-14978 ,
		-15137 ,
		-15286 ,
		-15426 ,
		-15557 ,
		-15679 ,
		-15791 ,
		-15893 ,
		-15986 ,
		-16069 ,
		-16143 ,
		-16207 ,
		-16261 ,
		-16305 ,
		-16340 ,
		-16364 ,
		-16379 ,
		-16384 ,
		-16379 ,
		-16364 ,
		-16340 ,
		-16305 ,
		-16261 ,
		-16207 ,
		-16143 ,
		-16069 ,
		-15986 ,
		-15893 ,
		-15791 ,
		-15679 ,
		-15557 ,
		-15426 ,
		-15286 ,
		-15137 ,
		-14978 ,
		-14811 ,
		-14635 ,
		-14449 ,
		-14256 ,
		-14053 ,
		-13842 ,
		-13623 ,
		-13395 ,
		-13160 ,
		-12916 ,
		-12665 ,
		-12406 ,
		-12140 ,
		-11866 ,
		-11585 ,
		-11297 ,
		-11003 ,
		-10702 ,
		-10394 ,
		-10080 ,
		-9760 ,
		-9434 ,
		-9102 ,
		-8765 ,
		-8423 ,
		-8076 ,
		-7723 ,
		-7366 ,
		-7005 ,
		-6639 ,
		-6270 ,
		-5897 ,
		-5520 ,
		-5139 ,
		-4756 ,
		-4370 ,
		-3981 ,
		-3590 ,
		-3196 ,
		-2801 ,
		-2404 ,
		-2006 ,
		-1606 ,
		-1205 ,
		-804 ,
		-402 ,
};

/**
 * returns the sin of an angle in degrees
 *
 * will signed to unsigned conversions be a problem here?
 * TODO test signed to unsigned conversion
 */
int sin2( unsigned int i ){
	return pgm_read_word(&sinTable[i % 256]);
}

/**
 * returns the cosine of a angle in degrees
 */
int cos2( unsigned int i ){
	return pgm_read_byte(&sinTable[(i + 90) % 256]);
}

struct Vector2 vector2Subtract( struct Vector2 *p1, struct Vector2 *p2 ){
	struct Vector2 p3;
	p3.x = p2->x - p1->x;
	p3.y = p2->y - p1->y;
	return p3;
}


float vector2Distance( struct Vector2 *p1, struct Vector2 *p2){
	return (float)sqrt(
		((p1->x + p2->x) ^ 2) +
		((p1->y + p2->y) ^ 2)
	);
}
